function [CNE, h, vN, qBL] = cornavparams_psiform_quat(CNE, h, vN, qBL, dp) %#codegen
%CORNAVPARAMS_PSIFORM_QUAT 利用导航参数误差修正INS导航参数，采用ψ形式导航误差方程，姿态采用四元数表示
%
% Input Arguments:
% # CNE: 3*3*m矩阵，修正前的位置矩阵，m为采样数，无单位
% # h: m*1向量，修正前的高度，单位m
% # vN: m*3矩阵，修正前的速度，单位m/s
% # qBL: m*4矩阵，修正前的姿态四元数，无单位
% # dp: m*10矩阵，导航参数误差，第1、2、10列元素为位置误差，第3列元素为高度误差，第4-6列元素为速度误差，
% 第7-9列元素为姿态误差，单位分别为rad、m、m/s、rad
%
% Output Arguments:
% # CNE: 3*3*m矩阵，修正后的位置矩阵，无单位
% # h: m*1向量，修正后的高度，单位m
% # vN: m*3矩阵，修正后的速度，单位m/s
% # qBL: m*4矩阵，修正后的姿态四元数，无单位
%
% References:
% #《应用导航算法工程基础》“导航参数的修正”

CNL = [0 1 0; 1 0 0; 0 0 -1];
dthetaN = dp(:, [1 2 10])';
phiL = CNL * (dthetaN+dp(:, 7:9)'); % REF1式9.27
% REF1式9.169-9.170
for i=1:size(dp, 1)
    qBL(i, :) = quatmultiply(rv2quat_5thod(phiL(:, i)), qBL(i, :));
    CNE(:, :, i) = CNE(:, :, i) * rv2dcm_5thod(dthetaN(:, i))';
    dvNNTilde = dp(i, 4:6) - cross(dthetaN(:, i)', vN(i, :)); % REF1式9.60
    vN(i, :) = vN(i, :) - dvNNTilde;
end
h = h - dp(:, 3);
end